/*************************************************************************
    Projet  -  Sport Collectif pour Robots
                             -------------------
    Auteur  -  Maxime Swinnen + équipe matuchifra
*************************************************************************/

//---------- Réalisation de la classe <Strategie> (fichier Strategie.cpp) --

//---------------------------------------------------------------- INCLUDE

//-------------------------------------------------------- Include système
using namespace std;
#include <unistd.h>

// added by MAtuchifra
#include <cmath>
// end add

#include "sys/time.h"

//------------------------------------------------------ Include personnel
#include "Strategie.h"
#include "../config.h"
#include "../util.h"

using namespace std;
using namespace cv;
using namespace scor;

//-------------------------------------------------------------- Variables
Vec2d afficheObjectifRobotG;
Vec2d afficheObjectifRobotD;

//------------------------------------------------------------- Constantes

//----------------------------------------------------------- Declarations

//----------------------------------------------------------- Types privés


//----------------------------------------------------------------- PUBLIC
//-------------------------------------------------------- Fonctions amies

//----------------------------------------------------- Méthodes publiques
// type Strategie::Méthode ( liste de paramètres )
// Algorithme :
//
//{
//} //----- Fin de Méthode

bool Strategie::LancerStrategie()
{
	if(strategieThreadLoop)
	{
		//le thread est deja lance
		return false;
	}
	strategieThreadLoop = true;

	LOG4CPLUS_INFO(loggerIA, "Lancement Strategie.");

	pthread_mutex_lock(&lockStopStrategieThread);

	//lancer le thread d'arret de strategie
	stopStrategieParams = new val_st<Strategie>;
	stopStrategieParams->pThis = this;
	stopStrategieParams->pFunc = this->stopStrategieThreadWrapper();
	if( pthread_create(&stopStrategieThreadId, NULL, &pThreadWrapper<Strategie>, static_cast<void*>(stopStrategieParams)) != 0 ) {
		//echec de la creation du thread
		return false;
	}

	//lancer le thread de strategie
	strategieParams = new val_st<Strategie>;
	strategieParams->pThis = this;
	strategieParams->pFunc = this->strategieThreadWrapper();
	if( pthread_create(&strategieThreadId, NULL, &pThreadWrapper<Strategie>, static_cast<void*>(strategieParams)) != 0 ) {
		//echec de la creation du thread
		return false;
	}
	return true;
} //----- Fin de <lancerStrategie>


int Strategie::ArreterStrategie()
{
	if(!strategieThreadLoop) {
		return false;
	}
	strategieThreadLoop = false;
	pthread_mutex_unlock(&lockStopStrategieThread);

	//Attendre la terminaison du thread d'analyse d'image
	int cr, stopCr;
	cr = pthread_join(stopStrategieThreadId, (void **)&stopCr);

	// Nettoyage
	delete stopStrategieParams;
	delete strategieParams;

	return cr;
} //----- Fin de <arreterStrategie>

//------------------------------------------------- Surcharge d'opérateurs
//Strategie & Strategie::operator = ( const Strategie & unStrategie )
//// Algorithme :
////
//{
//} //----- Fin de operator =


//-------------------------------------------- Constructeurs - destructeur


Strategie::Strategie (MsgQueue<DataIA>* msgQRG, MsgQueue<DataIA>* msgQRD, MsgQueue<DataVision>* msgQVis)
// Algorithme :
//
{
	defenseEnCours = false;

	LOG4CPLUS_TRACE(loggerIA, "Appel au constructeur de <Strategie>");

    msgQueueControleRG = msgQRG;
	msgQueueControleRD = msgQRD;
	msgQueueVision = msgQVis;

	distanceAncienObj = 0;

	compteurAberrations = 0;

	vecteurBallePrec[X] = 0;
	vecteurBallePrec[Y] = 0;

	vecteurBalleRef[X] = 0;
	vecteurBalleRef[Y] = 0;

	// Initialisation message comparaison objectif
	ancienMsgObjRD.posObjectif[0] = 0;
	ancienMsgObjRG.posObjectif[1] = 0;

	directionBut[X] = 0;
	directionBut[Y] = 1;
	vitesseBalle[X] = 0;
	vitesseBalle[Y] = 0;

	// Positions de replacement definies suivant les dimensions du terrain
	positionAvRobotGauche[X] = QUART_LARGEUR_TERRAIN_X;
	positionAvRobotGauche[Y] = (LONGUEUR_TERRAIN_Y)/4;

	positionArRobotGauche[X] = QUART_LARGEUR_TERRAIN_X;
	positionArRobotGauche[Y] = (LONGUEUR_TERRAIN_Y)/8;

	positionAvRobotDroite[X] = 3*QUART_LARGEUR_TERRAIN_X;
	positionAvRobotDroite[Y] = (LONGUEUR_TERRAIN_Y)/4;

	positionArRobotDroite[X] = 3*QUART_LARGEUR_TERRAIN_X;
	positionArRobotDroite[Y] = (LONGUEUR_TERRAIN_Y)/8;

	// add by matuchiFRA
	positionDefRobotGauche[X] = QUART_LARGEUR_TERRAIN_X - LONGUEUR_BRAS/2;
	positionDefRobotGauche[Y] = LIGNE_DE_BUT;
	dirDefRobotGauche[X] = 1;
	dirDefRobotGauche[Y] = 0;

	positionDefRobotDroite[X] = 3*QUART_LARGEUR_TERRAIN_X + LONGUEUR_BRAS/2;;
	positionDefRobotDroite[Y] = LIGNE_DE_BUT;
	dirDefRobotDroite[X] = -1;
	dirDefRobotDroite[Y] = 0;

	positionSoutienRobotGauche[X] = MOITIE_LARGEUR_TERRAIN_X - LONGUEUR_BRAS - DIAMETRE_ROBOT;
	positionSoutienRobotGauche[Y] = LIGNE_DE_BUT;
	positionSoutienRobotDroite[X] = MOITIE_LARGEUR_TERRAIN_X + LONGUEUR_BRAS + DIAMETRE_ROBOT;
	positionSoutienRobotDroite[Y] = LIGNE_DE_BUT;
	// end add

	strategieThreadLoop = false;

	/*timeval time;
	gettimeofday(&time, NULL);
	dateDernierEnvoiD = time.tv_sec*1000 + time.tv_usec/1000 + 0.5;
	dateDernierEnvoiG = time.tv_sec*1000 + time.tv_usec/1000 + 0.5;*/

	dateDernierEnvoiD = dateDernierEnvoiG = computeTimeStamp();

	if (pthread_mutex_init(&lockStopStrategieThread, NULL) != 0)
	{
		//error
	}
	if (pthread_mutex_init(&lockStrategieThreadLoop, NULL) != 0)
	{
		//error
	}
} //----- Fin de Strategie


Strategie::~Strategie ( )
// Algorithme :
//
{
	LOG4CPLUS_TRACE(loggerIA, "Appel au destructeur de <Strategie>");
    pthread_mutex_destroy(&lockStopStrategieThread);
    pthread_mutex_destroy(&lockStrategieThreadLoop);
} //----- Fin de ~Strategie


//------------------------------------------------------------------ PRIVE

//----------------------------------------------------- Méthodes protégées

//------------------------------------------------------- Méthodes privées

void Strategie::evaluer()
// Mode d'emploi :
//	Determine la bonne strategie a adopter suivant les conditions de jeu
{
	// STRATEGIE ATTAQUE
	// Conditions : Balle dans notre camp et immobile
	if( balleDansNotreCamp() && valeurVitesseBalle < 1)
	{
		defenseEnCours = false;
		attaquer();
	}
	// STRATEGIE DEFENSE
	// Conditions :
	// Direction du vecteur vitesse de la balle vers nos buts
	else if( balleVaVersNotreBut() &&
			(	(valeurVitesseBalle > 0.5 && balleDansNotreCamp() )
				|| (valeurVitesseBalle > 5 && ! balleDansNotreCamp() )) )
	{
		defendre();
	}
	// STRATEGIE REPLACEMENT
	// Nous considerons ici que lorsque nous ne sommes ni en defense,
	// ni en attaque, la strategie a adopter est le replacement
	else
	{
		defenseEnCours = false;
		replacer();
	}

} //----- Fin de <evaluer>


bool Strategie::envoyerMessageRobotGauche(DataIA messageAenvoyer)
{

	// Distance par rapport a ancien objectif
	/*distanceAncienObj = sqrt( 	(ancienMsgObjRG.posObjectif[0]-messageAenvoyer.posObjectif[0])*(ancienMsgObjRG.posObjectif[0]-messageAenvoyer.posObjectif[0])
							  + (ancienMsgObjRG.posObjectif[1]-messageAenvoyer.posObjectif[1])*(ancienMsgObjRG.posObjectif[1]-messageAenvoyer.posObjectif[1]));

	*/

	distanceAncienObj = distancePosObjectif(ancienMsgObjRG, messageAenvoyer);

	/*timeval time;
	long timeStamp;
	gettimeofday(&time, NULL);
	timeStamp = time.tv_sec*1000 + time.tv_usec/1000 + 0.5;*/

	long timeStamp = computeTimeStamp();


	if(distanceAncienObj > IA_SEUIL_ENVOI_NVL_OBJ && (timeStamp - dateDernierEnvoiG) > PERIODE_MIN_ENVOI)
	{

		dateDernierEnvoiG = timeStamp;
		msgQueueControleRG->push(messageAenvoyer);
		ancienMsgObjRG = messageAenvoyer;
		afficheObjectifRobotG = messageAenvoyer.posObjectif;

		return true;
	}

	return false;
} //----- Fin de <envoyerMessageRobotGauche>


bool Strategie::envoyerMessageRobotDroite(DataIA messageAenvoyer)
{
	// Distance par rapport a ancien objectif
	/*	distanceAncienObj = sqrt(	  (ancienMsgObjRD.posObjectif[0]-messageAenvoyer.posObjectif[0])*(ancienMsgObjRD.posObjectif[0]-messageAenvoyer.posObjectif[0])
									+ (ancienMsgObjRD.posObjectif[1]-messageAenvoyer.posObjectif[1])*(ancienMsgObjRD.posObjectif[1]-messageAenvoyer.posObjectif[1]));
	 */

	distanceAncienObj = distancePosObjectif(ancienMsgObjRD, messageAenvoyer);

	/*timeval time;
	long timeStamp;
	gettimeofday(&time, NULL);
	timeStamp = time.tv_sec*1000 + time.tv_usec/1000 + 0.5;
	*/

	long timeStamp = computeTimeStamp();

	if(distanceAncienObj > IA_SEUIL_ENVOI_NVL_OBJ && (timeStamp - dateDernierEnvoiD) > PERIODE_MIN_ENVOI)
	{
		dateDernierEnvoiD = timeStamp;
		msgQueueControleRD->push(messageAenvoyer);
		ancienMsgObjRD = messageAenvoyer;
		afficheObjectifRobotD = messageAenvoyer.posObjectif;

		return true;
	}

	return false;
} //----- Fin de <envoyerMessageRobotDroite>


int Strategie::validerObjectif(int idRobot)
// Mode d'emploi :
// Test la validite des positions objectifs envoyees aux robots
{
	assert(idRobot == ROBOT_GAUCHE || idRobot == ROBOT_DROITE);

	Vec2d positionObjectif(0,0);

	// Repere
	double origine = 0 + MARGE_SECURITE;
	double milieu = (double) MOITIE_LARGEUR_TERRAIN_X;

	// Variable temporaires permettant de renvoyer la validite
	// de la composante X de la position Objectif suivant le repere
	int inferieurOrigine = 0;
	int superieurOrigine = 0;

	if(idRobot == ROBOT_GAUCHE)
	{
		positionObjectif = msgObjectifRG.posObjectif;

		/* Si on teste pour le robot de gauche une abscisse inferieure a l'origine correspond a une abscisse
			hors des limites du terrain et une abscisse
			superieure au milieu correspond a une abscisse
			hors du couloir du robot */
		inferieurOrigine = POSITION_HORS_LIMITES;
		superieurOrigine = POSITION_HORS_COULOIR;
	}
	else if(idRobot == ROBOT_DROITE)
	{
		positionObjectif = msgObjectifRD.posObjectif;

		// Decalage du repere
		origine = double(MOITIE_LARGEUR_TERRAIN_X);
		milieu = double(LARGEUR_TERRAIN_X) - MARGE_SECURITE;

		/* Pour le robot de droite, une abscisse inferieur a l'origine correspond a une abscisse hors du couloir
			du robot, et une abscisse superieur au "milieu"
			correspond a une abscisse hors des limites du terrain */
		inferieurOrigine = POSITION_HORS_COULOIR;
		superieurOrigine = POSITION_HORS_LIMITES;
	}

	if(positionObjectif[Y] >= double(MOITIE_LONGUEUR_TERRAIN_Y)- MARGE_SECURITE)
	{
		return POSITION_HORS_MOITIE;
	}

	if(positionObjectif[X] < origine)
	{
		return inferieurOrigine;
	}

	if(positionObjectif[X] > milieu)
	{
		return superieurOrigine;
	}

	return POSITION_CORRECTE;
}

/**	Transmet aux robots les positions auxquelles ils doivent se placer lors de la strategie d'attaque **/
void Strategie::attaquer()
{
	/*
	// Determination du robot a envoyer suivant la position laterale de la balle

	if( balleAGauche() )
	{
		// La balle est dans la partie gauche du terrain : ROBOT GAUCHE va l'attaquer

		// Si la balle est dans la moitie gauche du couloir du robot, le robot
		// doit se placer sur la droite de la balle
		if(positionBalle.coord[X] < (double)QUART_LARGEUR_TERRAIN_X)
		//if(positionBalle.coord[X] < positionRobotGauche[X])
		{
			msgObjectifRG.posObjectif[X] = positionBalle.coord[X] + DISTANCE_TIR; // le robot doit se placer à droite de la balle
			msgObjectifRG.posObjectif[Y] = positionBalle.coord[Y];// - DISTANCE_TIR;

			msgObjectifRG.dirObjectif[X] = -1;
			msgObjectifRG.dirObjectif[Y] = 0;

			msgObjectifRG.typeStrategie = ATTAQUE_GAUCHE;
		}
		else
		{
			msgObjectifRG.posObjectif[X] = positionBalle.coord[X] - DISTANCE_TIR; // le robot doit se placer à gauche de la balle
			msgObjectifRG.posObjectif[Y] = positionBalle.coord[Y];// - DISTANCE_TIR;

			msgObjectifRG.dirObjectif[X] = 0;
			msgObjectifRG.dirObjectif[Y] = 1;

			msgObjectifRG.typeStrategie = ATTAQUE_DROITE;
		}

		if(envoyerMessageRobotGauche(msgObjectifRG))
		{
			LOG4CPLUS_DEBUG(loggerIA, "[IA] ATTAQUE OBJ RG X " << afficheObjectifRobotG[0]);
			LOG4CPLUS_DEBUG(loggerIA, "[IA] ATTAQUE OBJ RG Y " << afficheObjectifRobotG[1]);
		}

		msgObjectifRD.posObjectif = positionDefRobotDroite; //positionArRobotDroite; change by matuchiFRA
		msgObjectifRD.dirObjectif[0] = 0;
		msgObjectifRD.dirObjectif[1] = 1;

		msgObjectifRD.typeStrategie = REPLACEMENT;

		if(envoyerMessageRobotDroite(msgObjectifRD))
		{
			LOG4CPLUS_DEBUG(loggerIA, "[IA] REPLACEMENT OBJ RD X " << afficheObjectifRobotD[0]);
			LOG4CPLUS_DEBUG(loggerIA, "[IA] REPLACEMENT OBJ RD Y " << afficheObjectifRobotD[1]);
		}

	}
	else
	{

		// // La balle est dans la partie droite du terrain : ROBOT DROITE va l'attaquer

		if(positionBalle.coord[X] < (double)(3*LARGEUR_TERRAIN_X)/4)
		//if(positionBalle.coord[X] < positionRobotDroite.coord[X])
		{
			msgObjectifRD.posObjectif[X] = positionBalle.coord[X] + DISTANCE_TIR; // on tape par la droite
			msgObjectifRD.posObjectif[Y] = positionBalle.coord[Y]; // - DISTANCE_TIR;


			msgObjectifRD.dirObjectif[X] = 0;
			msgObjectifRD.dirObjectif[Y] = 1;

			msgObjectifRD.typeStrategie = ATTAQUE_GAUCHE;
		}
		else
		{
			msgObjectifRD.posObjectif[X] = positionBalle.coord[X] - DISTANCE_TIR; // on tape par la gauche
			msgObjectifRD.posObjectif[Y] = positionBalle.coord[Y]; // - DISTANCE_TIR;

			msgObjectifRD.dirObjectif[X] = 0;
			msgObjectifRD.dirObjectif[Y] = 1;

			msgObjectifRD.typeStrategie = ATTAQUE_DROITE;
		}


		if(envoyerMessageRobotDroite(msgObjectifRD))
		{
			LOG4CPLUS_DEBUG(loggerIA, "[IA] ATTAQUE OBJ RD X " << afficheObjectifRobotD[0]);
			LOG4CPLUS_DEBUG(loggerIA, "[IA] ATTAQUE OBJ RD Y " << afficheObjectifRobotD[1]);
		}

		msgObjectifRG.posObjectif = positionDefRobotGauche; //positionArRobotGauche; change by matuchiFRA
		//msgObjectifRG.dirObjectif[0] = 0;
		//msgObjectifRG.dirObjectif[1] = 1;
		msgObjectifRG.dirObjectif = dirDefRobotGauche;

		msgObjectifRG.typeStrategie = REPLACEMENT;

		if(envoyerMessageRobotGauche(msgObjectifRG))
		{
			LOG4CPLUS_DEBUG(loggerIA, "[IA] REPLACEMENT OBJ RG X " << afficheObjectifRobotG[0]);
			LOG4CPLUS_DEBUG(loggerIA, "[IA] REPLACEMENT OBJ RG Y " << afficheObjectifRobotG[1]);
		}
	}*/
	
	// added by MAtuchifra
	// Determination du robot a envoyer suivant la position laterale de la balle

	if( balleAGauche() )
	{
		// La balle est dans la partie gauche du terrain : ROBOT GAUCHE va l'attaquer

		// Si la balle est dans la moitie gauche du couloir du robot, le robot
		// doit se placer sur la droite de la balle
		if(positionBalle.coord[X] < (double)QUART_LARGEUR_TERRAIN_X)
		//if(positionBalle.coord[X] < positionRobotGauche[X])
		{
			msgObjectifRG.posObjectif[X] = positionBalle.coord[X] + DISTANCE_TIR; // le robot doit se placer à droite de la balle
			msgObjectifRG.posObjectif[Y] = positionBalle.coord[Y] + this->ChoisirVerticalDistanceTir();// - DISTANCE_TIR;

			msgObjectifRG.dirObjectif[X] = -1;
			msgObjectifRG.dirObjectif[Y] = 0;

			msgObjectifRG.typeStrategie = ATTAQUE_GAUCHE;
		}
		else
		{
			msgObjectifRG.posObjectif[X] = positionBalle.coord[X] - DISTANCE_TIR; // le robot doit se placer à gauche de la balle
			msgObjectifRG.posObjectif[Y] = positionBalle.coord[Y] + this->ChoisirVerticalDistanceTir();// - DISTANCE_TIR;

			msgObjectifRG.dirObjectif[X] = 0;
			msgObjectifRG.dirObjectif[Y] = 1;

			msgObjectifRG.typeStrategie = ATTAQUE_DROITE;
		}

		if(envoyerMessageRobotGauche(msgObjectifRG))
		{
			LOG4CPLUS_DEBUG(loggerIA, "[IA] ATTAQUE OBJ RG X " << afficheObjectifRobotG[0]);
			LOG4CPLUS_DEBUG(loggerIA, "[IA] ATTAQUE OBJ RG Y " << afficheObjectifRobotG[1]);
		}

		msgObjectifRD.posObjectif = positionDefRobotDroite; //positionArRobotDroite; change by matuchiFRA
		msgObjectifRD.dirObjectif[0] = 0;
		msgObjectifRD.dirObjectif[1] = 1;

		msgObjectifRD.typeStrategie = REPLACEMENT;

		if(envoyerMessageRobotDroite(msgObjectifRD))
		{
			LOG4CPLUS_DEBUG(loggerIA, "[IA] REPLACEMENT OBJ RD X " << afficheObjectifRobotD[0]);
			LOG4CPLUS_DEBUG(loggerIA, "[IA] REPLACEMENT OBJ RD Y " << afficheObjectifRobotD[1]);
		}

	}
	else
	{

		// // La balle est dans la partie droite du terrain : ROBOT DROITE va l'attaquer

		if(positionBalle.coord[X] < (double)(3*LARGEUR_TERRAIN_X)/4)
		//if(positionBalle.coord[X] < positionRobotDroite.coord[X])
		{
			msgObjectifRD.posObjectif[X] = positionBalle.coord[X] + DISTANCE_TIR; // on tape par la droite
			msgObjectifRD.posObjectif[Y] = positionBalle.coord[Y] + this->ChoisirVerticalDistanceTir(); // - DISTANCE_TIR;


			msgObjectifRD.dirObjectif[X] = 0;
			msgObjectifRD.dirObjectif[Y] = 1;

			msgObjectifRD.typeStrategie = ATTAQUE_GAUCHE;
		}
		else
		{
			msgObjectifRD.posObjectif[X] = positionBalle.coord[X] - DISTANCE_TIR; // on tape par la gauche
			msgObjectifRD.posObjectif[Y] = positionBalle.coord[Y] + this->ChoisirVerticalDistanceTir(); // - DISTANCE_TIR;

			msgObjectifRD.dirObjectif[X] = 0;
			msgObjectifRD.dirObjectif[Y] = 1;

			msgObjectifRD.typeStrategie = ATTAQUE_DROITE;
		}


		if(envoyerMessageRobotDroite(msgObjectifRD))
		{
			LOG4CPLUS_DEBUG(loggerIA, "[IA] ATTAQUE OBJ RD X " << afficheObjectifRobotD[0]);
			LOG4CPLUS_DEBUG(loggerIA, "[IA] ATTAQUE OBJ RD Y " << afficheObjectifRobotD[1]);
		}

		msgObjectifRG.posObjectif = positionDefRobotGauche; //positionArRobotGauche; change by matuchiFRA
		msgObjectifRG.dirObjectif[0] = 0;
		msgObjectifRG.dirObjectif[1] = 1;

		msgObjectifRG.typeStrategie = REPLACEMENT;

		if(envoyerMessageRobotGauche(msgObjectifRG))
		{
			LOG4CPLUS_DEBUG(loggerIA, "[IA] REPLACEMENT OBJ RG X " << afficheObjectifRobotG[0]);
			LOG4CPLUS_DEBUG(loggerIA, "[IA] REPLACEMENT OBJ RG Y " << afficheObjectifRobotG[1]);
		}
	}
	// end add
} //----- Fin de <attaquer>


/**	Transmet aux robots les positions auxquelles ils doivent se placer lors de la strategie de defense **/
void Strategie::defendre()
{
	/*
	// Coordonnees balle
	double absBalle = positionBalle.coord[X];
	double ordBalle = positionBalle.coord[Y];

	// Coordonnees robots
	double absRobotG = positionRobotGauche.coord[X];
	double ordRobotG = positionRobotGauche.coord[Y];

	double absRobotD = positionRobotDroite.coord[X];
	double ordRobotD = positionRobotDroite.coord[Y];

	// Equation de droite de la balle
	// f(x) = coeff * x + constante
	double coeff = vitesseBalle[1]/vitesseBalle[0];
	double constante = ordBalle - coeff*absBalle;

	// Coordonnees projection robot gauche
	Vec2d coordIntersectionRG(0,0);

	coordIntersectionRG[0] = (absRobotG + coeff*(ordRobotG - constante)) / (1 + coeff*coeff);
	coordIntersectionRG[1] = constante + coeff*(absRobotG + coeff*(ordRobotG - constante)) / (1 + coeff*coeff);

	// Coordonnees projection robot droite
	Vec2d coordIntersectionRD(0,0);

	coordIntersectionRD[0] = (absRobotD + coeff*(ordRobotD - constante)) / (1 + coeff*coeff);
	coordIntersectionRD[1] = constante + coeff*(absRobotD + coeff*(ordRobotD - constante)) / (1 + coeff*coeff);


	// Envoi des messages aux robots

	// Robot gauche
	msgObjectifRG.posObjectif = coordIntersectionRG;

	msgObjectifRG.dirObjectif[0] = 0;
	msgObjectifRG.dirObjectif[1] = 1;

	msgObjectifRG.typeStrategie = DEFENSE;

	if(validerObjectif(ROBOT_GAUCHE) > 0)
	{
		if(envoyerMessageRobotGauche(msgObjectifRG))
		{
			LOG4CPLUS_DEBUG(loggerIA, "[IA] DEFENSE OBJ RG X " << afficheObjectifRobotG[0]);
			LOG4CPLUS_DEBUG(loggerIA, "[IA] DEFENSE OBJ RG Y " << afficheObjectifRobotG[1]);
		}
	}

	// Robot droite

	msgObjectifRD.posObjectif = coordIntersectionRD;

	msgObjectifRD.dirObjectif[0] = 0;
	msgObjectifRD.dirObjectif[1] = 1;

	msgObjectifRD.typeStrategie = DEFENSE;

	if(validerObjectif(ROBOT_DROITE) > 0)
	{
		if(envoyerMessageRobotDroite(msgObjectifRD))
		{
			LOG4CPLUS_DEBUG(loggerIA, "[IA] DEFENSE OBJ RD X " << afficheObjectifRobotD[0]);
			LOG4CPLUS_DEBUG(loggerIA, "[IA] DEFENSE OBJ RD Y " << afficheObjectifRobotD[1]);
		}
	}

*/


	/* Nouvelle stratégie by matuchiFRA
	 	On suppose que les 2 robots sont sur leur ligne de but.
		Il faut évaluer la trajectoire de la balle, et estimer où elle va couper la ligne de but.
		En fonction du point d'intersection, RG ou RD va bouger
	*/


	//if(! defenseEnCours)
	//{
		// on récupère le point d'intersection prévue de la balle avec notre ligne de but
		Vec2d pointIntersection = pointIntersectionLigneDeBut(positionPrecedenteBalle.coord, positionBalle.coord);

		if(pointIntersection[X] < 0)
			return;

		// sinon la position de défense est valide
		defenseEnCours = true;
		LOG4CPLUS_INFO(loggerIA, "Position défense valide, X=" << pointIntersection[X] << ", Y=" << pointIntersection[Y]);

		if( pointIntersection[X] >=0 && pointIntersection[X] < MOITIE_LARGEUR_TERRAIN_X )
		{
			// le robot gauche doit se charger de l'interception
			msgObjectifRG.posObjectif = pointIntersection;
			msgObjectifRG.dirObjectif = dirDefRobotGauche;
			msgObjectifRG.typeStrategie = DEFENSE;
			if(validerObjectif(ROBOT_GAUCHE) > 0)
			{
				if(envoyerMessageRobotGauche(msgObjectifRG))
				{
					LOG4CPLUS_DEBUG(loggerIA, "[IA] DEFENSE OBJ RG : X=" << afficheObjectifRobotG[X] << ", Y=" << afficheObjectifRobotG[Y]);
				}
				// le robot droit va en soutien du robot gauche
				msgObjectifRD.posObjectif = positionSoutienRobotDroite;
				envoyerMessageRobotDroite(msgObjectifRD);
			}
		}
		else
		{
			// le robot droit doit se charger de l'interception
			msgObjectifRD.posObjectif = pointIntersection;
			msgObjectifRD.dirObjectif = dirDefRobotDroite;
			msgObjectifRD.typeStrategie = DEFENSE;
			if(validerObjectif(ROBOT_DROITE) > 0)
			{
				if(envoyerMessageRobotDroite(msgObjectifRD))
				{
					LOG4CPLUS_DEBUG(loggerIA, "[IA] DEFENSE OBJ RD : X=" << afficheObjectifRobotD[X] << ", Y=" << afficheObjectifRobotD[Y]);
				}

				// le robot gauche va en soutien du robot droite
				msgObjectifRG.posObjectif = positionSoutienRobotGauche;
				envoyerMessageRobotGauche(msgObjectifRG);
			}
		}
	//}


} //----- Fin de <defendre>



/**	Transmet aux robots les positions auxquelles ils doivent se placer lors de la strategie de replacement **/
void Strategie::replacer()
{
	// Affectation des positions suivant la position de la balle
	/*
	if(positionBalle.coord[0] <= MOITIE_LARGEUR_TERRAIN_X )
	{
		// Si balle du cote gauche, robot gauche en position avant
		// et robot droite en position arriere
		//msgObjectifRG.posObjectif = positionAvRobotGauche;
		// msgObjectifRD.posObjectif = positionArRobotDroite;
	}
	else
	{
		// Si balle du cote droit, robot gauche en position arriere
		// et robot droite en position avant
		msgObjectifRG.posObjectif = positionArRobotGauche;

		msgObjectifRD.posObjectif = positionAvRobotDroite;
	}*/

	/* Changement : les 2 robots se replacent proche de leur cage, tels 2 gardiens de buts,
		avec leur raquette parallèle à la ligne de but pour avoir une plus grande allonge pour stopper la balle */
	msgObjectifRG.posObjectif = positionDefRobotGauche;
	msgObjectifRD.posObjectif = positionDefRobotDroite;

	msgObjectifRG.dirObjectif[0] = 1;
	msgObjectifRG.dirObjectif[1] = 0;

	msgObjectifRG.typeStrategie = REPLACEMENT;

	if(envoyerMessageRobotGauche(msgObjectifRG))
	{
		LOG4CPLUS_DEBUG(loggerIA, "[IA] REPLACEMENT OBJ RG X " << afficheObjectifRobotG[0]);
		LOG4CPLUS_DEBUG(loggerIA, "[IA] REPLACEMENT OBJ RG Y " << afficheObjectifRobotG[1]);
	}

	msgObjectifRD.dirObjectif[0] = -1;
	msgObjectifRD.dirObjectif[1] = 0;

	msgObjectifRD.typeStrategie = REPLACEMENT;

	if(envoyerMessageRobotDroite(msgObjectifRD))
	{
		LOG4CPLUS_DEBUG(loggerIA, "[IA] REPLACEMENT OBJ RD X " << afficheObjectifRobotD[0]);
		LOG4CPLUS_DEBUG(loggerIA, "[IA] REPLACEMENT OBJ RD Y " << afficheObjectifRobotD[1]);
	}

} //----- Fin de <replacer>



bool Strategie::detecterAberrations(const Vec2d nouveauVecteurVitesse)
{
	if( ! ( nouveauVecteurVitesse[X] == 0 && nouveauVecteurVitesse[Y] == 0
			&& vecteurBalleRef[X] == 0 && vecteurBalleRef[Y] == 0) )
	{
			if(nouveauVecteurVitesse.dot(vecteurBalleRef) < IA_SEUIL_ABERRATION_BALLE)
			{
				if(nouveauVecteurVitesse.dot(vecteurBallePrec) >= IA_SEUIL_ABERRATION_BALLE
						|| (nouveauVecteurVitesse[X] == 0 && nouveauVecteurVitesse[Y] ==0))
				{
					++compteurAberrations;

					if(compteurAberrations == IA_SEUIL_NVLLE_TRAJECTOIRE )
					{
						compteurAberrations = 0;
						vecteurBalleRef = nouveauVecteurVitesse;
					}
				}
				else
				{
					compteurAberrations = 1;
				}
			}
			else
			{
				compteurAberrations = 0;
			}
	}
	else
	{
		compteurAberrations = 0;
	}

	vecteurBallePrec = nouveauVecteurVitesse;

	return (compteurAberrations > 0 ? true:false);
}


void Strategie::strategieThread()
{
	// Boucle d'appel
	DataVision msgDeVision;
	bool quit;

	for(;;)
	{

		// Reception message Vision
		msgQueueVision->wait_and_pop(msgDeVision);

		// Recuperation des positions robots et de la balle
		positionRobotGauche = msgDeVision.posRobots[0];
		positionRobotDroite = msgDeVision.posRobots[1];

		// Test si fin d'applicaiton
		pthread_mutex_lock(&lockStrategieThreadLoop);
		quit = !strategieThreadLoop;
		pthread_mutex_unlock(&lockStrategieThreadLoop);
		if(quit)
		{
			break;
		}

		// Calcul de la vitesse de la balle
		positionPrecedenteBalle = positionBalle;
		positionBalle = msgDeVision.posBalle;
		if(calculerVitesse(positionPrecedenteBalle, positionBalle, vitesseBalle) && positionBalle.coord[0] != 0)
		{
			valeurVitesseBalle = sqrt(vitesseBalle[0]*vitesseBalle[0] + vitesseBalle[1]*vitesseBalle[1]);

//			valeurVitesseBalle = (int)valeurVitesseBalle;

			if(valeurVitesseBalle < 0.5)
			{
				valeurVitesseBalle = 0;
				vitesseBalle[X] = 0;
				vitesseBalle[Y] = 0;
			}
		}

		// Test aberrations
		if(valeurVitesseBalle > 100 || positionBalle.coord[X] == 0)
		{
			continue;
		}

		if(!detecterAberrations(vitesseBalle))
		{
			evaluer();
			//ancienMsgObjRD = msgObjectifRD;
			//ancienMsgObjRG = msgObjectifRG;
		}
	}
}


void Strategie::stopStrategieThread()
{
	DataVision msgQuit;
	//attente sur semaphore
	pthread_mutex_lock(&lockStopStrategieThread);

	// Notification de fin d'application
	pthread_mutex_lock(&lockStrategieThreadLoop);
	strategieThreadLoop = false;
	pthread_mutex_unlock(&lockStrategieThreadLoop);

	// Simulation d'un message en provenance de la Vision pour eviter
	// le blocage en reception de message dans le thread de strategie
	msgQueueVision->push(msgQuit);

	//Attendre la terminaison du thread de strategie
	pthread_join(strategieThreadId, NULL);

} //----- Fin de <stopStrategieThread>

// ####################################### Add by Matuchifra Team ############################################


/** Retourne la distance Euclidienne entre les positions objectifs de deux DataIA **/
double Strategie::distancePosObjectif(DataIA d1, DataIA d2)
{
	return sqrt(  (d1.posObjectif[0]-d2.posObjectif[0])*(d1.posObjectif[0]-d2.posObjectif[0])
				+ (d1.posObjectif[1]-d2.posObjectif[1])*(d1.posObjectif[1]-d2.posObjectif[1]));
}

bool Strategie::balleDansNotreCamp()
{
	return positionBalle.coord[Y] < MOITIE_LONGUEUR_TERRAIN_Y ;
}

bool Strategie::balleVaVersNotreBut()
{
	return vitesseBalle.dot(directionBut) < 0;
}

bool Strategie::balleAGauche()
{
	return positionBalle.coord[X] < double(MOITIE_LARGEUR_TERRAIN_X);
}

bool Strategie::balleADroite()
{
	return positionBalle.coord[X] > double(MOITIE_LARGEUR_TERRAIN_X);
}

/**
 * Estime la trajectoire de la balle et renvoie le point en lequel la balle devrait couper notre ligne de but
 * Les rebonds sur les murs sont pris en compte
 * @param trajBalle : vecteur représentant la trajectoire de la balle (doit être orienté vers notre ligne de but)
 */
Vec2d & Strategie::pointIntersectionLigneDeBut(const Vec2d & prevPosBall, const Vec2d & nextPosBalle)
{
	Vec2d pointIntersection;
	pointIntersection[X] = -1;
	pointIntersection[Y] = LIGNE_DE_BUT; // dans tous les cas on cherche l'intersection avec notre ligne de but

	Vec2d trajBalle;
	trajBalle[X] = nextPosBalle[X] - prevPosBall[X];
	trajBalle[Y] = nextPosBalle[Y] - prevPosBall[Y];

	if(trajBalle[Y] >= 0)
		return pointIntersection;

	if(abs(trajBalle[X]) < 0.01)
	{
		//pointIntersection[X] = nextPosBalle[X];
		pointIntersection[X] = -1;
		return pointIntersection;
	}

	// équation de droite le la trajectoire de la balle (T) : y = mx+p
	double m = trajBalle[Y] / trajBalle[X];
	double p = nextPosBalle[Y] - m*nextPosBalle[X];

	/* on cherche le point d'intersection de cette droite avec la droite représentant
	   notre ligne de but d'équation (L) : y = LIGNE_DE_BUT */
	double x = ( (double) LIGNE_DE_BUT - p) / m;


	if( x >= 0 && x <= LARGEUR_TERRAIN_X )
	{
		pointIntersection[X] = x;
		// la balle va aller directement vers notre but sans rebondir sur un mur
		return pointIntersection;
	}

	// sinon la balle va toucher un mur, il faut prendre en compte le rebond
	if(x < 0)
	{
		//return pointIntersection; // juste pour tester, remove that later

		// la balle va rebondir sur le mur de gauche : il faut calculer l'angle de rebond

		Vec2d ptIntersectionMurGauche;
		ptIntersectionMurGauche[X] = 0;
		ptIntersectionMurGauche[Y] = p; // on sait déjà que la droite va couper le mur en ce point

		Vec2d mur_gauche;
		mur_gauche[X] = 0;
		mur_gauche[Y] = 1;
		Vec2d tballe = -trajBalle;
		double scalaire = mur_gauche[X]*tballe[X] + mur_gauche[Y]*tballe[Y];
		double cos_theta = scalaire / (norme(mur_gauche) * norme(tballe));
		double theta = acos(cos_theta); // thêta est l'angle d'incidence
		if(theta < 0 || theta > M_PI/2)
		{
				LOG4CPLUS_INFO(loggerIA, "theta hors limite mur gauche : theta = "<<theta<<" équation de la droite : "<<m<<"*x+"<<p<<"  x calcule = "<<x);
				return pointIntersection;
		}
		LOG4CPLUS_INFO(loggerIA, "theta gauche= " << theta);


		// Il faut maintenant trouver l'équation de la droite après rebond
		// On fait une rotation de 180-2*theta
		// Le centre de la rotation est le point d'intersection avec le mur
		double a_r = - (M_PI-2*theta);

		Vec2d ptApresRotation;
		ptApresRotation[X] = cos(a_r)*(prevPosBall[X] - ptIntersectionMurGauche[X]) - sin(a_r)*(prevPosBall[Y] - ptIntersectionMurGauche[Y]) + ptIntersectionMurGauche[X];
		ptApresRotation[Y] = sin(a_r)*(prevPosBall[X] - ptIntersectionMurGauche[X]) + cos(a_r)*(prevPosBall[Y] - ptIntersectionMurGauche[Y]) + ptIntersectionMurGauche[Y];

		return pointIntersectionLigneDeBut(ptIntersectionMurGauche, ptApresRotation);
	}
	else
	{
		// la balle va rebondir sur le mur de droite
		Vec2d ptIntersectionMurDroite;
		ptIntersectionMurDroite[X] = LARGEUR_TERRAIN_X;
		ptIntersectionMurDroite[Y] = m*LARGEUR_TERRAIN_X + p;

		if(ptIntersectionMurDroite[Y] < 0 || ptIntersectionMurDroite[Y] > LONGUEUR_TERRAIN_Y)
		{
			LOG4CPLUS_INFO(loggerIA, "Intersection hors limite mur droite : Y = " << ptIntersectionMurDroite[Y] << " équation de la droite : "<<m<<"*x+"<<p<<"  x calcule = "<<x);
			return pointIntersection;
		}

		// la balle va rebondir sur le mur de gauche : il faut calculer l'angle de rebond
		Vec2d mur_droite;
		mur_droite[X] = 0;
		mur_droite[Y] = 1;
		Vec2d tballe = -trajBalle;
		double scalaire = mur_droite[X]*tballe[X] + mur_droite[Y]*tballe[Y];
		LOG4CPLUS_INFO(loggerIA, "scalaire droite = " << scalaire);
		double cos_theta = scalaire / (norme(mur_droite) * norme(tballe));
		LOG4CPLUS_INFO(loggerIA, "cos_theta droite = " << cos_theta);
		double theta = acos(cos_theta); // thêta est l'angle d'incidence

		if(theta < 0 || theta > M_PI/2)
		{
				LOG4CPLUS_INFO(loggerIA, "theta hors limite mur droite : theta = "<<theta<<" équation de la droite : "<<m<<"*x+"<<p<<"  x calcule = "<<x);
				return pointIntersection;
		}

		LOG4CPLUS_INFO(loggerIA, "theta droite = " << theta);


		// Il faut maintenant trouver l'équation de la droite après rebond
		// On fait une rotation de 180-2*theta
		// Le centre de la rotation est le point d'intersection avec le mur
		double a_r = M_PI-2*theta;

		Vec2d ptApresRotation;
		ptApresRotation[X] = cos(a_r)*(prevPosBall[X] - ptIntersectionMurDroite[X]) - sin(a_r)*(prevPosBall[Y] - ptIntersectionMurDroite[Y]) + ptIntersectionMurDroite[X];
		ptApresRotation[Y] = sin(a_r)*(prevPosBall[X] - ptIntersectionMurDroite[X]) + cos(a_r)*(prevPosBall[Y] - ptIntersectionMurDroite[Y]) + ptIntersectionMurDroite[Y];

		return pointIntersectionLigneDeBut(ptIntersectionMurDroite, ptApresRotation);
	}
}


/* added by MAtuchifra */
Tir_Type Strategie::ChoisirTirType() {

	// Dans cette fonction, il faut communiquer les positions des robots adversaires
	// Selon les positions renvoyer le type de tir
	
	// identifier 3 distances:
	// 1. distance de robot adverse droit par rapport au mur
	// 2. distance entre les 2 robots
	// 3. distance de robot adverse droit par rapport au mu

	double distanceRDAWall = positionRobotAdvDroite[X];
	double distanceRGWall = LARGEUR_TERRAIN_X - positionRobotAdvGauche[X];
	double distanceEntreRobotsAdverses = positionRobotAdvGauche[X] - positionRobotAdvDroite[X];

	// 3 cas particuliers :
	// 1. position de la balle ~ position du robot droite
	// 2. position de la balle ~ position du robot gauche
    // 3. position de la balle ~ moitié du terrain
	double epsilon = 5;
	double difference;

	// il faut identifier aussi quel robot va tirer
	if(balleADroite())
	{

		// cas 1 : pas du tir direct
		if((difference = abs((double)(positionRobotAdvDroite[X] - positionBalle.coord[X])) ) < epsilon )
		{
			if(distanceEntreRobotsAdverses > distanceRDAWall)
			{
				return Court;
			}
			else
			{
				return Long;
			}
		}

		else {
			if(distanceEntreRobotsAdverses > distanceRDAWall && distanceEntreRobotsAdverses > distanceRGWall)
			{
				return Defaut;
			}
			else
			{
				if(distanceRDAWall > distanceRGWall )
				{
					return Long;
				}
				else
				{
					return Court;
				}
			}
		}
	}
	else{
		// cas 2 : pas du tir direct
		if((difference = abs((double)(positionRobotAdvGauche[X] - positionBalle.coord[X])) ) < epsilon )
		{
			if(distanceEntreRobotsAdverses > distanceRGWall)
			{
				return Long;
			}
			else
			{
				return Court;
			}
		}

		else {
			if(distanceEntreRobotsAdverses > distanceRDAWall && distanceEntreRobotsAdverses > distanceRGWall)
			{
				return Defaut;
			}
			else
			{
				if(distanceRDAWall > distanceRGWall )
				{
					return Long;
				}
				else
				{
					return Court;
				}
			}
		}
	}
}

double Strategie::ChoisirVerticalDistanceTir() {
	Tir_Type tirType = ChoisirTirType();
	switch(tirType)
	{
		case Long:
			return (double)DISTANCE_VERTICAL_TIR_LONG;
		case Court:
			return (double)DISTANCE_VERTICAL_TIR_COURTE;
		default:
			return (double)DISTANCE_VERTICAL_TIR;
	}
}
/* end add */
